function [KScal, dfBiasS, dPSS0, dKScal1, residual, KScalResid, dfBiasSResid, dPSS0Resid, dKScal1Resid] = accposcalib( pulseRate, g, KScalExt )
%ACCPOSCALIB 按分立级多位置标定算法对加速度计进行标定
%
% Tips:
% # 不能标定出来的参数将被置为NaN
%
% Input Arguments:
% # pulseRate: m*3矩阵，第i行为第i个位置上的加速度计脉冲输出率（单位^/s），第j列为第j个加速度计的输出
% # g: 标量，标定点的重力加速度，单位m/s^2，默认为9.8
% # KScalExt: 3*1向量，外部给出的标度因数，当位置数不够而不能计算出某个加速度计的标度因数时使用该值，单位(^/s)/(m/s^2)，默认全为NaN
%
% Output Arguments:
% # KScal: 3*1向量，标度因数，单位(^/s)/(m/s^2)
% # dfBiasS: 3*1向量，零偏，单位m/s^2
% # dPSS0: 3*3矩阵，安装误差（失准角），单位rad
% # dKScal1: 3*1向量，二次项系数，单位1/(m/s^2)
% # residual: m*3向量，为各位置上各加速度计的标定残差，单位^/s
% # KScalResid: m*3矩阵，各位置标定残差对应的标度因数误差，单位(^/s)/(m/s^2)
% # dfBiasSResid: m*3矩阵，各位置标定残差对应的零偏误差，单位m/s^2
% # dPSS0Resid: 3*3*m数组，各位置标定残差对应的安装误差误差，单位rad
% # dKScal1Resid: m*3矩阵，各位置标定残差对应的二次项系数误差，单位1/(m/s^2)
%
% Assumptions and Limitations:
% # 各位置上惯组必须有一个轴朝上或者朝下，另两个轴在水平方向（本函数根据加速度输出自动判断竖直轴及水平轴）
% # 本函数忽略了标定设备定位及调平误差
%
% References:
% # 《应用导航算法工程基础》“加速度计三元组误差参数标定算法”

if nargin < 3
    KScalExt = NaN(3, 1);
end
if nargin < 2
    g = 9.8;
end

m = size(pulseRate, 1);
% 根据脉冲输出自动判断惯组坐标系各轴的姿态（水平、向上、向下）
fB = zeros(m, 3);
for i=1:m
    [~, I] = max(abs(pulseRate(i, :)), [], 2);
    if pulseRate(i, I) > 0
        fB(i, I) = 1;
    else
        fB(i, I) = -1;
    end
end
% 构造系数矩阵A
H = zeros(m, 5, 3);
for i=1:m
    H(i, :, 1) = [1 fB(i, 1) fB(i, 2) fB(i, 3) fB(i, 1)^2];
    H(i, :, 2) = [1 fB(i, 1) fB(i, 2) fB(i, 3) fB(i, 2)^2];
    H(i, :, 3) = [1 fB(i, 1) fB(i, 2) fB(i, 3) fB(i, 3)^2];
end
% 求解方程组
p = zeros(5, 3); % 依次为dfBiasS、KScal、dPSS01、dPSS02、dKScal1
dp = NaN(5, 3, m);
residual = NaN(m, 3);
for i=1:3
    [G, zeroRowIdx] = pinv_nonzero(H(:, :, i));
    p(:, i) = G * pulseRate(:, i);
    residual(:, i) = pulseRate(:, i) - H(:, :, i)*p(:, i);
    for j=1:m
        dp(:, i, j) = G(:, j) * residual(j, i);
    end
    p(zeroRowIdx, i) = NaN; % 将无法标定出的参数置无效值
end
% 求解加速度计误差参数
[KScal, dfBiasS, dPSS0, dKScal1] = arrpar2structpar(p, g, KScalExt, false);
KScalResid = NaN(m, 3);
dfBiasSResid = NaN(m, 3);
dPSS0Resid = NaN(3, 3, m);
dKScal1Resid = NaN(m, 3);
for i=1:m
    [KScalResid_i, dfBiasSResid_i, dPSS0Resid(:, :, i), dKScal1Resid_i] = arrpar2structpar(dp(:, :, i), g, KScal, true);
    KScalResid(i, :) = KScalResid_i';
    dfBiasSResid(i, :) = dfBiasSResid_i';
    dKScal1Resid(i, :) = dKScal1Resid_i';
end
end

function [KScal, dfBiasS, dPSS0, dKScal1] = arrpar2structpar(p, g, KScalExt, pIsDelta)
KScal = [p(2, 1) p(3, 2) p(4, 3)]' / g;
if pIsDelta
    KScal0 = KScalExt;
else
    KScal(isnan(KScal)) = KScalExt(isnan(KScal));
    KScal(isinf(KScal)) = KScalExt(isinf(KScal));
    KScal0 = KScal;
end
p([1, 3:5], 1) = p([1, 3:5], 1) / (KScal0(1, 1)*g);
p([1:2, 4:5], 2) = p([1:2, 4:5], 2) / (KScal0(2, 1)*g);
p([1:3, 5], 3) = p([1:3, 5], 3) / (KScal0(3, 1)*g);
dfBiasS = [p(1, 1) p(1, 2) p(1, 3)]' * g;
dPSS0 = [0 p(3, 1) p(4, 1)
    p(2, 2) 0 p(4, 2)
    p(2, 3) p(3, 3) 0]';
dKScal1 = [p(5, 1) p(5, 2) p(5, 3)]' / g;
end